/**************************************************************************
 * @file data_pack_handle.c
 * @author BryanHe(贺璧) (532352406@qq.com)
 * @brief 
 * @version 0.1
 * @date 2025-04-07
 * 
 * @copyright Copyright (c) 2025
 * 
 *************************************************************************/
#include "../inc/data_pack_handle.h"

// CRC计算函数（使用CRC-16/CCITT-FALSE算法，多项式0x1021，初始值0xFFFF）
uint16_t calculate_crc(const uint8_t *dat, uint32_t length) {
    uint16_t crc = 0xFFFF;
    for (uint32_t i = 0; i < length; i++) {
        crc ^= (uint16_t)dat[i] << 8;
        for (int j = 0; j < 8; j++) {
            if (crc & 0x8000) {
                crc = (crc << 1) ^ 0x1021;
            } else {
                crc <<= 1;
            }
        }
    }
    return crc;
}

// 数据转义函数
static void escape_data(const uint8_t *input, uint32_t input_len, uint8_t *output, uint32_t *output_len) {
    uint32_t j = 0;
    uint8_t dat;

    for (uint32_t i = 0; i < input_len; i++) {
        dat = input[i];
        if (dat == USR_D_PKG_HEAD) {
            output[j++] = USR_D_ESCAPE_ID;
            output[j++] = 0x01;
        } else if (dat == USR_D_PKG_END) {
            output[j++] = USR_D_ESCAPE_ID;
            output[j++] = 0x02;
        } else if (dat == USR_D_ESCAPE_ID) {
            output[j++] = USR_D_ESCAPE_ID;
            output[j++] = 0x03;
        } else {
            output[j++] = dat;
        }
    }
    *output_len = j;
}

// 数据解转义函数
static int unescape_data(const uint8_t *input, uint32_t input_len, uint8_t *output, uint32_t *output_len) {
    uint32_t j = 0;

    if(input_len > USR_D_PACK_MAX_PACKAGE_LEN)return -1;

    for (uint32_t i = 0; i < input_len; ) {
        if (input[i] == USR_D_ESCAPE_ID) {
            if (i + 1 >= input_len) return -1;
            switch (input[i+1]) {
                case 0x01: output[j++] = USR_D_PKG_HEAD; break;
                case 0x02: output[j++] = USR_D_PKG_END; break;
                case 0x03: output[j++] = USR_D_ESCAPE_ID; break;
                default: return -1;
            }
            i += 2;
        } else {
            output[j++] = input[i];
            i++;
        }
    }
    *output_len = j;
    return 0;
}

// 数据打包函数
int pack_data(const uint8_t *dat, uint8_t data_len, uint8_t *packet) {
    // 构造原始数据块（数据 + 2字节CRC）
    uint8_t original_block[USR_D_PACK_ORIGINAL_PK_MAX];  
    uint16_t crc;
    uint8_t escaped_block[USR_D_PACK_MAX_PACKAGE_LEN];  // 最大可能长度
    uint32_t escaped_len;

    if(USR_D_PACK_MAX_DAT_PAYLOAD<data_len) return -1;/*Failed*/

    memcpy(original_block, dat, data_len);
    
    crc = calculate_crc(original_block, data_len);
    original_block[data_len] = crc >> 8;
    original_block[1 + data_len] = crc & 0xFF;
    
    // 转义处理
    escape_data(original_block, 2 + data_len, escaped_block, &escaped_len);
    
    // 构造完整数据包
    packet[0] = USR_D_PKG_HEAD;
    memcpy(&packet[1], escaped_block, escaped_len);
    packet[1 + escaped_len] = USR_D_PKG_END;
    
    return (escaped_len + 2);
}

// 数据解包函数
int unpack_data(const uint8_t *packet, uint32_t packet_len, uint8_t *dat, uint8_t *data_len, uint16_t *crc) {
    uint32_t escaped_len = packet_len - 2;
    uint8_t original_block[USR_D_PACK_ORIGINAL_PK_MAX];
    uint32_t original_len;
    uint16_t calculated_crc;

    // 基本长度检查
    if (packet_len < 4 || packet[0] != USR_D_PKG_HEAD || packet[packet_len-1] != USR_D_PKG_END) {
        return -1;/**Failed */
    }
    
    // 解转义处理
    if (-1 == unescape_data(&packet[1], escaped_len, original_block, &original_len)) {
        return -1;/**Failed */
    }
    
    // 解析基本字段
    if (original_len < 3) {
        return -1;/**Failed */
    }
    
    // 提取字段
    *data_len = original_len-2;
    memcpy(dat, &original_block, *data_len);
    *crc = (original_block[*data_len] << 8) | original_block[1 + *data_len];
    
    // 验证CRC
    calculated_crc = calculate_crc(original_block, *data_len);

    if(calculated_crc == *crc){
        return 0; /**Success */
    }else{
        return -1;/**Failed */
    }
}

/* 使用示例：
// 打包示例
uint8_t data[] = {0x01, 0xAA, 0xFF, 0x55};
uint8_t packet[256];
uint32_t packet_len = usr_pack_data(data, 4, packet);

// 解包示例
uint8_t unpacked_data[256];
uint8_t data_len;
uint16_t crc;
if (0 == usr_unpack_data(packet, packet_len, unpacked_data, &data_len, &crc)) {
    // 处理解包成功的数据
}
*/